In [1]:
%pylab qt
In [2]:
%matplotlib inline
%connect_info
Try to mask out HUD/UI elements by detection regions that slowly change from frame to frame
In [3]:
images = array(images)
diffs = images[1:] - images[:-1]
mask = std(diffs, 0)
thresh = 0.3;
mask = cv2.GaussianBlur(mask, (0,0), 2)
mask[mask < mask.max() * thresh] = 0
mask[mask >= mask.max() * thresh] = 1
imshow(mask)
In [128]:
import cv2
In [190]:
mask2 = cv2.medianBlur(array(mask, dtype=np.float32), 5)
mask2 = cv2.erode(mask2, ones((3,3)))
imshow(mask2)
Out[190]:
In [53]:
imshow(images[4], cmap=cm.gray)
plot(points[:, 0], points[:, 1], 'r.')
Out[53]:
In [602]:
plot(mv[:,0], mv[:,2], '.')
Out[602]:
In [1614]:
from sklearn import linear_model
X = np.vstack((mv[:, 0]**2, mv[:, 0], ones(mv.shape[0]))).T
y = mv[:, 2]
# Robustly fit linear model with RANSAC algorithm
model_ransac = linear_model.RANSACRegressor(linear_model.LinearRegression(fit_intercept=False))
model_ransac.fit(X, y)
inlier_mask = model_ransac.inlier_mask_
outlier_mask = np.logical_not(inlier_mask)
a, b, c = model_ransac.estimator_.coef_
a, b, c
Out[1614]:
In [1615]:
%matplotlib qt
X_in = mv[inlier_mask, 0]
X_out = mv[outlier_mask, 0]
y_in = mv[inlier_mask, 2]
y_out = mv[outlier_mask, 2]
plot(X_in, y_in, '.')
plot(X_out, y_out, '.')
t = linspace(X_in.min(), X_in.max(), 100)
plot(t, a*t**2 + b*t + c)
x_min = -b/(2*a)
a*x_min**2 + b*x_min + c
Out[1615]:
In [1672]:
control = [10e3, 15e3, 20e3,
25e3, 26e3, 27e3, 28e3, 29e3, 29.5e3,
30e3]
speed = [9.740644336872279/3, 36.030645952209511/3, 37.124312115088088,
101.34249310780383, 123.63380553013994, 80.687738743971806*2, 95.34390238190602*2, 110.64445509500386*2, 78.85052787064636*10,
162.92974488164242*10]
plot(control, speed, '.-')
Out[1672]:
In [571]:
%pylab inline
from sklearn import linear_model
x_mins = []
y_mins = []
for idx in range(len(M)):
mv = M[idx]
s = S[idx]
#plot(mv[:,0], mv[:,2], '.')
X = np.vstack((mv[:, 0]**2, mv[:, 0], ones(mv.shape[0]))).T
y = mv[:, 2] / s
# Robustly fit linear model with RANSAC algorithm
model_ransac = linear_model.RANSACRegressor(linear_model.LinearRegression(fit_intercept=False))
model_ransac.fit(X, y)
inlier_mask = model_ransac.inlier_mask_
outlier_mask = np.logical_not(inlier_mask)
a, b, c = model_ransac.estimator_.coef_
a, b, c
X_in = mv[inlier_mask, 0]
X_out = mv[outlier_mask, 0]
y_in = mv[inlier_mask, 2] / s
y_out = mv[outlier_mask, 2] / s
plot(X_in, y_in, '.')
#plot(X_out, y_out, '.')
t = linspace(X_in.min(), X_in.max(), 100)
plot(t, a*t**2 + b*t + c)
x_min = -b/(2*a)
x_mins.append(x_min)
y_mins.append(a*x_min**2 + b*x_min + c)
y_mins = array(y_mins)
In [572]:
plot(y_mins, '.')
Out[572]:
In [577]:
from scipy.optimize import minimize
import sys
def FrameDelta(X, rot_hz, epsilon, s=1, dt=1.0/60.0, w=1280, f=1280):
X = X - w/2 + epsilon
phi_dot = 2*pi * rot_hz
Theta1 = arctan(X / f)
Theta2 = Theta1 + s*dt*phi_dot
return f*(tan(Theta2) - tan(Theta1))
def GetInliers(X, y):
X = np.vstack((X**2, X, ones(X.shape[0]))).T
# Robustly fit linear model with RANSAC algorithm
model_ransac = linear_model.RANSACRegressor(linear_model.LinearRegression(fit_intercept=False))
model_ransac.fit(X, y)
return model_ransac.inlier_mask_
class MinFun (object):
def __init__(self, X, y, s):
self.X = X
self.y = y
self.s = s
def __call__(self, args):
rot_hz = args[0]
epsilon = args[1]
f = args[2]
y_hat = FrameDelta(self.X, rot_hz, epsilon, self.s, f=f)
diff = y_hat - self.y
#return diff.dot(diff)
return sum((abs(diff) + 1))
tick = True
thetas = []
for idx in xrange(len(M)):
#for idx in xrange(90,110):
#idx = 90
s = S[idx]
X = M[idx][:, 0]
y = M[idx][:, 2]
c = C[idx]
W = 1280
#inliers = GetInliers(X, y)
#X_in = X[inliers]
#y_in = y[inliers]
X_in = X
y_in = y
minfun = MinFun(X_in, y_in, s)
x0 = [sqrt(0.0577), eps_hat, f_hat]
#x0 = [sqrt(0.0577), 0, 1000]
if tick:
res = minimize(lambda x: minfun([x[0], eps_hat, f_hat]), x0, method='Powell')
else:
res = minimize(minfun, x0, method='Powell')
#print '{} rot_hz={:.4f}, eps={:.4f}, f={:.4f}'.format(res.success, res.x[0], res.x[1], res.x[2])
#print res
if res.success:
thetas.append([idx, s, c, res.x[0], res.x[1], res.x[2]])
if idx % 10 == 0:
print idx
else:
print 'Fail at', idx
#N = 200
#t = linspace(0, W, N)
#y_lin = FrameDelta(t, res.x[0], res.x[1], s, f=res.x[2])
#plot(t, y_lin)
#plot(X_in, y_in, '.')
thetas = array(thetas)
if not tick:
eps_hat = mean(thetas[:120, 4])
f_hat = mean(thetas[:120, 5])
print 'eps_hat={}, f_hat={}'.format(eps_hat, f_hat)
N = 130
y_mins_norm = thetas[:N, 3].max()*y_mins/y_mins.max()
plot(thetas[:N, 0], thetas[:N, 3], '.')
plot(y_mins_norm[:N], '.')
diff = abs(thetas[:N, 3] - y_mins_norm[:N])
print 'avg diff is', sum(diff / N)
print 'std diff is', std(diff)
In [653]:
#plot(thetas[:N, 2], thetas[:N, 3])
from scipy.optimize import curve_fit
def small_control(x, a, c, d):
return a*np.exp(c*x) + d
popt, pcov = curve_fit(small_control, C[4:,0], y_mins_norm[4:], p0=(.001, 1e-3, 0))
#print popt, pcov
t = linspace(1000, 30000, 100)
#plot(C[4:], y_mins_norm[4:])
#plot(t, small_control(t, *popt))
y_hat = small_control(C[4:], *popt)[:,0]
print max(abs(y_hat - y_mins_norm[4:]))
plot(C[4:], y_hat)
plot(C[4:], y_mins_norm[4:])
Out[653]:
In [693]:
def GetControl (phi, popt):
a, c, d = popt
inflection = y_mins_norm[4:].max()
fastest = y_mins_norm.max()
if phi < inflection:
return log((phi - d)/a)/c
else:
exp_control = log((inflection - d)/a)/c
max_control = 32000
coin = random.rand() * (fastest - inflection)
if coin + inflection < phi:
return max_control
else:
return exp_control
print popt
print y_mins_norm[4:].max()
print y_mins_norm.max()
In [698]:
small_control(0, a, c, d)
Out[698]:
In [697]:
%store M
%store C
%store S